#include "WPILib.h"
#include "Robot2489.h"
#include "Ballistic2D.h"

Robot2489::Robot2489(void) {
	
	//Initialize motors/relays
	InitJaguar();
	InitEncoders();
	InitPIDControllers();
	InitSpike();
	InitJoystick();
	InitSolenoid();
	InitLimitSwitch();
	InitImage();
	InitIntake();
	
	//Initialize drive controller
	myRobot = new RobotDrive(leftMotor, rightMotor);
	
	//Driver station and axis camera
	station = DriverStation::GetInstance();
	screen = DriverStationLCD::GetInstance();
	enhancedIO = &station->GetEnhancedIO();
	
	//Stuff 
	myRobot->SetExpiration(0.1);
	timer = new Timer();
	utilityTimer = new Timer();
	debugFile = fopen("debug.txt", "w");
	//autoTimer = new Timer();
	
	bo = new Ballistic2D();
	// Now set our paramters speed, angle, etc
}

void Robot2489::InitJaguar(void)
{
	leftMotor         = new Jaguar(LEFTSIDECAR , LEFTMOTOR);
	rightMotor        = new Jaguar(RIGHTSIDECAR, RIGHTMOTOR);
	conveyorBeltMotor = new Jaguar(LEFTSIDECAR , CONVEYORBELT);
	turretMotor       = new Jaguar(LEFTSIDECAR, TURRETMOTOR);
	bottomTurretWheel = new Jaguar(RIGHTSIDECAR, BOTTOMTURRETWHEEL);
}

void Robot2489::InitEncoders(void)
{
	leftEncoder  = new Encoder(LEFTSIDECAR , A_PORT, LEFTSIDECAR , B_PORT);
	rightEncoder = new Encoder(RIGHTSIDECAR, A_PORT, RIGHTSIDECAR, B_PORT, true);
	leftEncoder->SetPIDSourceParameter(Encoder::kDistance);
	rightEncoder->SetPIDSourceParameter(Encoder::kDistance);
	leftEncoder ->SetDistancePerPulse(10 * PI/180.0 * SPROCKET_RATIO * WHEEL_RADIUS);
	rightEncoder->SetDistancePerPulse(PI/180.0 * SPROCKET_RATIO * WHEEL_RADIUS);

	gearToothCounter = new GearTooth(RIGHTSIDECAR, GEARTOOTHPORT, false);
	gearToothCounter->Start();
	
}

void Robot2489::InitSpike(void)
{
	rampTool = new Relay(RIGHTSIDECAR, RAMPTOOLPORT);
}

void Robot2489::InitPIDControllers(void)
{
	leftMotorController = new PIDController(DRIVE_P, DRIVE_I, DRIVE_D,  //PID constants
											leftEncoder,                //input source
											leftMotor);                 //output source
	rightMotorController = new PIDController(DRIVE_P, DRIVE_I, DRIVE_D, //PID constants
											 rightEncoder,              //input source
											 rightMotor);               //output source
}

void Robot2489::InitJoystick(void){
	leftStick  = new Joystick(LEFTSTICK);
	rightStick = new Joystick(RIGHTSTICK);
	manipStick = new Joystick(MANIPSTICK);
	
	gameStick = new Joystick(GAMESTICK);
}
void Robot2489::InitSolenoid(void){
	ringLight = new Solenoid(RINGLIGHTPORT);
}

void Robot2489::InitLimitSwitch(void)
{
	//ballDetectLimit  = new DigitalInput(RIGHTSIDECAR, BALLDETECT);
	rightTurretLimit = new DigitalInput(RIGHTSIDECAR, RIGHTLIMIT);
	leftTurretLimit  = new DigitalInput(LEFTSIDECAR, LEFTLIMIT);
	rampToolStop = new DigitalInput(LEFTSIDECAR, RAMPTOOLLIMIT);
}

void Robot2489::InitImage(void)
{
	camera = &AxisCamera::GetInstance("10.24.89.11");
	particles = new vector<particlePoint>;
}

void Robot2489::InitIntake(void){
#ifdef INTAKE_CONTROL
	compressor = new Compressor(RIGHTSIDECAR, PVAVLE, RIGHTSIDECAR, PRELAY);
	compressor->Start();
	proximtySensorTop = new DigitalInput(RIGHTSIDECAR, PORTOP);
	proximtySensorBottom = new DigitalInput(RIGHTSIDECAR, PORBOT);
	solenoidTopOn = new Solenoid(SOLTON);
	solenoidTopOff = new Solenoid(SOLTOF);
	solenoidBottomOn = new Solenoid(SOLBON);
	solenoidBottomOff = new Solenoid( SOLBOF);
	topRetract = 0;
	botRetract =0;
	runningIntake =0;
#endif
}


START_ROBOT_CLASS(Robot2489);
